/*
 * main.cpp
 *
 *  Created on: 14 nov. 2012
 *      Author: Gwen
 */
#include "CDatapoolServer.h"
#include "Accelerometre.h"
#include "Motors.h"
#include "Servos.h"
#include "Radar.h"
#include "StereoPicture.h"
#include "StereoGrabber.h"
#include "Gps.h"
#include <iostream>
#include <sstream>
#include <signal.h>

//CDatapoolServer * server;
Accelerometre * accelerometre;
Motors * motors;
Servos * servos;
Radar * radar;
Gps * gps;

bool ShallStop = false;

void siginthandler(int param)
{
  std::cout << "User pressed Ctrl+C" << std::endl;
  ShallStop = true;
}


int main(int argc, char **argv)
{
	signal(SIGINT, siginthandler);

	int currentPositionX = 0;
	int currentPositionY = 0;
	double currentHeading = 0;

	//server = new CDatapoolServer();
	//accelerometre = new Accelerometre();
	motors = new Motors(38, 1, B19200, 10, 0, 0);
	servos = new Servos(38, 2, B4800);
	radar = new Radar(currentPositionX, currentPositionY,currentHeading);
	//gps = new Gps(1, B57600);

	//server->Run();
	//accelerometre->Run();

	servos->Run();
	radar->Run();
	motors->Run();
	//gps->Run();

	//servos->m_PanServo->m_Consigne = 1500;
	
	motors->m_LeftMotor->m_Consigne = 1.;
	motors->m_RightMotor->m_Consigne = 1.;

	//servos->m_PanServo->SetServoSpeed(15);

	while(false == ShallStop)
	{
		boost::this_thread::sleep(boost::posix_time::milliseconds(200));
	}

	//delete accelerometre;
	delete motors;
	delete servos;
	delete radar;
	//delete gps;
	//delete server;

	std::cout << "that's all falk!" << std::endl;

	return 0;
}
